// stl相关库
#include <iostream>
#include <vector>
#include <string>
#include <mutex>

// Ros相关库
#include <ros/ros.h>
#include <geometry_msgs/Point.h>
#include <acupoint_msgs/AcupointImagePixel.h>




#define FILTER_KERNEL 3

struct Point2D{
    float x;
    float y;
    Point2D(){
        x=0;
        y=0;
    }
};

std::mutex m;
ros::Publisher filter_pub;
acupoint_msgs::AcupointImagePixel acu_pixel_filter_msg;

// 穴位像素坐标滤波数组
std::vector<std::vector<Point2D>> origin_data(FILTER_KERNEL);
int flag=-1;


// 像素坐标回调函数
void pixelCallback(const acupoint_msgs::AcupointImagePixelConstPtr &msg){
    
    if(msg->name.size()!=acu_pixel_filter_msg.name.size()){
        acu_pixel_filter_msg.name.resize(msg->name.size());
        acu_pixel_filter_msg.pixels.resize(msg->name.size());
        for(int i=0;i<FILTER_KERNEL;++i){
            origin_data[i].resize(msg->name.size(),Point2D());
        }
        for(int i=0;i<msg->name.size();++i) acu_pixel_filter_msg.name[i]=msg->name[i];
        flag=-1;
    }

    // 如果没有第一帧，则将滤波数组初始化为第一帧数据
    if(flag==-1){
        for(int i=0;i<FILTER_KERNEL;++i){
            for(int j=0;j<acu_pixel_filter_msg.name.size();++j){
                origin_data[i][j].x=msg->pixels[j].x;
                origin_data[i][j].y=msg->pixels[j].y;
            }
        }
        flag=1;
    }
    else{
        for(int j=0;j<acu_pixel_filter_msg.name.size();++j){
            origin_data[flag][j].x=msg->pixels[j].x;
            origin_data[flag][j].y=msg->pixels[j].y;
        }
        flag=(flag+1)%FILTER_KERNEL;
    }

    std::vector<Point2D> temp(msg->name.size(),Point2D());
    for(int i=0;i<FILTER_KERNEL;++i){
        for(int j=0;j<acu_pixel_filter_msg.name.size();++j){
            temp[j].x+=origin_data[i][j].x;
            temp[j].y+=origin_data[i][j].y;
        }
    }

    for(int j=0;j<acu_pixel_filter_msg.name.size();++j){
        temp[j].x/=FILTER_KERNEL;
        temp[j].y/=FILTER_KERNEL;
        acu_pixel_filter_msg.pixels[j].x=temp[j].x;
        acu_pixel_filter_msg.pixels[j].y=temp[j].y;
    }
    // 发布滤波后的像素坐标
    acu_pixel_filter_msg.header.stamp=ros::Time::now();
    filter_pub.publish(acu_pixel_filter_msg);
}


int main(int argc, char *argv[])
{
    ros::init(argc, argv, "acupoint_pixel_filter_node");
    ros::NodeHandle nh;

    filter_pub=nh.advertise<acupoint_msgs::AcupointImagePixel>("/acupoint/acu_filter_image_pixel",1);
    ros::Subscriber pixel_sub=nh.subscribe<acupoint_msgs::AcupointImagePixel>("/acupoint/acu_origin_image_pixel",1,pixelCallback);
    ros::spin();
    return 0;
}
